#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include "visualization_msgs/Marker.h"
#include "sensor_msgs/PointCloud2.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
/*
geometry_msgs/TransformStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/Transform transform
  geometry_msgs/Vector3 translation
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation
    float64 x
    float64 y
    float64 z
    float64 w
*/
/*
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance
*/

ros::Publisher odom_pub, map_pub;
ros::Timer visual_map;
double size_x,size_y,init_x,init_y,map_width;

void vicon_callback(const geometry_msgs::TransformStamped::ConstPtr & msg){
	nav_msgs::Odometry odom_;
	odom_.pose.pose.position.x = msg-> transform. translation.x;
	odom_.pose.pose.position.y = msg-> transform. translation.y;
	odom_.pose.pose.position.z = msg-> transform. translation.z;
	
	odom_.pose.pose.orientation = msg-> transform.rotation;
	odom_.header.stamp = ros::Time::now();
	odom_.header.frame_id = "world";
	odom_pub.publish(odom_);
	
}

void map_callback(const ros::TimerEvent & e){
    // Draw line 1
//    int id = 0;
//    for(double i = -init_x ; i < size_x - init_x; i+=map_width){
//        visualization_msgs::Marker  map_list;
//        map_list.header.frame_id = "world";
//        map_list.header.stamp = ros::Time::now();
//        map_list.ns = "map_boundary";
//        map_list.id = id++;
//        map_list.action = visualization_msgs::Marker::ADD;
//        map_list.pose.orientation.w = 1.0;
//        map_list.type = visualization_msgs::Marker::CUBE;
//        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
//        map_list.scale.x = map_width;
//        map_list.scale.y = map_width;
//        map_list.scale.z = map_width;
//        // Line list is red
//        map_list.color.r = 0.5;
//        map_list.color.g = 0.6;
//        map_list.color.b = 1.0;
//        map_list.color.a = 1.0;
//        // Create the vertices for the points and lines
//        geometry_msgs::Point p;
//        p.x = i;
//        p.y = -init_y;
//        p.z = 0;
//        map_list.pose.position = p;
//        map_pub.publish(map_list);
//
//
//        p.x = i;
//        p.y = size_y-init_y;
//        p.z = 0;
//        map_list.pose.position = p;
//        map_pub.publish(map_list);
//    }
//
//    // Draw line 1
//    for(double i = -init_y ; i < size_y - init_y; i+=map_width){
//        visualization_msgs::Marker  map_list;
//        map_list.header.frame_id = "world";
//        map_list.header.stamp = ros::Time::now();
//        map_list.ns = "map_boundary";
//        map_list.id = id++;
//        map_list.action = visualization_msgs::Marker::ADD;
//        map_list.pose.orientation.w = 1.0;
//        map_list.type = visualization_msgs::Marker::CUBE;
//        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
//        map_list.scale.x = map_width;
//        map_list.scale.y = map_width;
//        map_list.scale.z = map_width;
//        // Line list is red
//        map_list.color.r = 0.5;
//        map_list.color.g = 0.6;
//        map_list.color.b = 1.0;
//        map_list.color.a = 1.0;
//        // Create the vertices for the points and lines
//        geometry_msgs::Point p;
//        p.x = -init_x;
//        p.y = i;
//        p.z = 0;
//        map_list.pose.position = p;
//        map_pub.publish(map_list);
//
//        p.x = size_x-init_x;
//        p.y = i;
//        p.z = 0;
//        map_list.pose.position = p;
//        map_pub.publish(map_list);
//    }
    pcl::PointXYZ pt_random;
    pcl::PointCloud<pcl::PointXYZ> pc;
    for(double i = -init_x ; i <= size_x - init_x; i+=map_width){
        // Create the vertices for the points and lines
        geometry_msgs::Point p;
        pt_random.x = i;
        pt_random.y = -init_y;
        pt_random.z = 0;
        pc.points.push_back(pt_random);


        pt_random.x = i;
        pt_random.y = size_y-init_y;
        pt_random.z = 0;
        pc.points.push_back(pt_random);
    }

    // Draw line 1
    for(double i = -init_y ; i <= size_y - init_y; i+=map_width){
        geometry_msgs::Point p;
        pt_random.x = -init_x;
        pt_random.y = i;
        pt_random.z = 0;
        pc.points.push_back(pt_random);

        pt_random.x = size_x-init_x;
        pt_random.y = i;
        pt_random.z = 0;
        pc.points.push_back(pt_random);
    }
    pc.width = pc.points.size();
    pc.height = 1;
    pc.is_dense = true;
    sensor_msgs::PointCloud2 globalMap_pcd;
    pcl::toROSMsg(pc, globalMap_pcd);
    globalMap_pcd.header.frame_id = "world";
    map_pub.publish(globalMap_pcd);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "vicon_repub");
	ros::NodeHandle nh("~");
	ros::Subscriber vicon_sub = nh.subscribe<geometry_msgs::TransformStamped>("/vicon/vehicle/vehicle", 1, vicon_callback);
	nh.param<double>("/map/size_x",size_x,5.0);
    nh.param<double>("/map/size_y",size_y,5.0);
    nh.param<double>("/map/init_x",init_x,5.0);
    nh.param<double>("/map/init_y",init_y,5.0);
    nh.param<double>("/map/map_width",map_width,0.2);
	odom_pub = nh.advertise<nav_msgs::Odometry>("/vehicle/odom",1);
    map_pub = nh.advertise<sensor_msgs::PointCloud2>("/map_boundary",1);
	visual_map = nh.createTimer(ros::Duration(0.1), map_callback);
	ros::spin();
}
